#include "Controler.h"
#include <math.h>
#include "ReadSenser.h"
#include "MS42DDC.h"
#include "VofaFireData.h"
#include "USART_HMI.h"
#include "KeyInput.h"
#include "CalibrationSensor.h"
#include "Battery.h"

// 本文件实现所有逻辑状态及压力闭环
#define MinDelta 0.001

ControlerStatus_e state = Ready;
uint8_t PressureDepletionCounter = 0; // 压力耗尽计数，用于判断阀门是否关闭

uint8_t ErrorStateJudge(void)
{
    // static uint8_t prejudge = 0;
    // for (uint8_t i = 0; i < 2; i++)
    // {
    //     if (GetEdgeDown(i))
    //     {
    //         state = Error;
    //         // prejudge++;
    //     }
    // }
    // if (prejudge && (GetKeyStateAll() == 0))
    // {
    //     prejudge++;
    // }
    // else
    // {
    //     prejudge = 0;
    // }
    // if (prejudge > 4)
    // {
    //     state = Error;
    //     prejudge = 0;
    // }
}

float GetTarget(void)
{
    switch (HMIGetStatus())
    {
    case HMI_Model:
        return GetCurrentTargetPressure();
        break;
    case HMI_Manual:
        return HMIGetData();
        break;
    default:
        return 0;
        break;
    }
}

static float GetControlerKp(float press)
{
    static float kp = 80;
    if (press > 12)
    {
        return 0;
    }
    return -(kp / 12) * press + kp;
}

/**
 * @brief 控制器
 *
 */
static void Controler(void)
{
    float speed = GetControlerKp(ReadSenserData_float()) * GetCurrentInstrumentKpParam() * (GetTarget() - ReadSenserData_float());
    MS42SpeedModeSetSpeed_Default(speed);

    MS42UARTTransmitCallback_Default(); // 电机控制
}

static void ControlStop(void)
{
    MS42SpeedModeSetSpeed_Default(0);
    MS42UARTTransmitCallback_Default(); // 电机控制
}

ControlerStatus_e GetControlerStatus(void)
{
    return state;
}

void ReadyStateFunc(void)
{
    // if (GetTarget() > ReadSenserData_float())
    // {
    //     state = IncreasePressure;
    // }
    // else
    // {
    //     state = ReducePressure;
    // }
    // Controler();
    if (state == Ready)
    {
        switch (HMIGetStatus())
        {
        case HMI_Stop:

            break;
        case HMI_GoZero:
        case HMI_PreFill:
            if (fabsf(ReadSenserData_float()) < 0.05) // 认为已经打开泄压阀
            {
                state = GoZero;
                HMIGoZeroStart();
            }
            break;
        case HMI_Model:
            PressureDepletionCounter = 100;
            if (fabsf(ReadSenserData_float()) < 0.05 && GetKeyState(0) == 0) // 认为已经回零完毕
            {
                state = IncreasePressure;
            }
            // else
            // {
            //     state = Ready; // 重新回零和开阀门
            //     HMISetStatus(HMI_GoZero);
            //     HMIOpenValve();
            // }
            break;
        case HMI_Manual:
            PressureDepletionCounter = 100;
            if (fabsf(ReadSenserData_float()) < 0.05 && GetKeyState(0) == 0) // 认为已经回零完毕
            {
                state = IncreasePressure; // 手动模式也考虑回零问题
            }
            break;
        case HMI_Drain:
        case HMI_Fill:
            state = ContinueIncrease; // 换液模式和排气模式进入持续加压状态，用于排空液体/气体
        default:

            break;
        }
    }
}

void GoZeroStateFunc(void)
{
    // 检测开关状态
    if (GetKeyState(0)) // 0是后开关
    {
        // 未按下
        MS42SpeedModeSetSpeed_Default(-30);
    }
    else
    {
        // 按下
        MS42SpeedModeSetSpeed_Default(0);
        state = Ready;
        HMIGoZeroFinish();
    }
    MS42UARTTransmitCallback_Default(); // 电机控制
}

void IncreasePressureStateFunc(void)
{
    if (PressureDepletionCounter > 0)
    {
        PressureDepletionCounter--;
    }
    if (PressureDepletionCounter == 0)
    {
        if (ReadSenserData_float() < 0.1)
        {
            // 认为是无效加压
            HMIValveCheck();
            state = Ready;
        }
    }
    if (UpdatePressure(ReadSenserData_float()) && (ReadSenserData_float() < 14 || ReadADCData_float() < 6))
    {
        Controler();
    }
    else
    {
        state = Ready;
        // HMIOpenValve();
        HMIMeasureFinish();
    }
    ErrorStateJudge();
}

void ContinueIncreaseStateFunc(void)
{
    MS42SpeedModeSetSpeed_Default(30); // 持续加压状态，电机以最高速度持续加压
    MS42UARTTransmitCallback_Default(); // 电机控制

    if (!GetKeyState(1)) // 1是前开关
    {
        // 按下，认为是停止加压
        state = Ready;
        HMIDrainFinish();
    }
}

void ReducePressureStateFunc(void)
{
    if (GetTarget() > ReadSenserData_float() + MinDelta)
    {
        state = IncreasePressure;
    }
    Controler();
    ErrorStateJudge();
}

void ErrorStateFunc(void)
{
    state = Error;
    // 出错之后不执行任何指令
    MS42SpeedModeSetSpeed_Default(0);
    MS42UARTTransmitCallback_Default(); // 电机控制
}

// 100ms执行一次
uint8_t ControlerCallback(void)
{
    if (HMIGetStatus() != HMI_Stop)
    {
        switch (state)
        {
        case Ready:
            // 就绪状态，等待指令到来后跳转不同的状态机
            ReadyStateFunc();
            break;
        case GoZero:
            // 回零状态，电机以最高速度返回零点。返回零点之前不接受处理指令
            GoZeroStateFunc();
            break;
        case IncreasePressure:
        case ReducePressure:
            // 加压状态，电机按照设定加压，接受处理指令
            IncreasePressureStateFunc();
            break;
        case ContinueIncrease:
            // 持续加压状态，电机以最高速度持续加压，直到达到停止条件
            ContinueIncreaseStateFunc();
            break;
        case Error:
            // 错误状态
            ErrorStateFunc();
            break;
        default:
            break;
        }
        return 1;
    }
    else
    {
        ControlStop();
        state = Ready;
    }
}
